#include <iostream>
#include <cmath>
#include <list>
#include <cppunit/CompilerOutputter.h>
#include <cppunit/extensions/TestFactoryRegistry.h>
#include <cppunit/ui/text/TestRunner.h>
#include <boost/numeric/mtl/mtl.hpp>
#include <CGAL/Random.h>

#include "fmm.h"
#include "isoline.h"
#include "geodesics.h"
#include "randompoint.h"
#include "gcvt.h"
#include "backtracking.h"

using namespace std;
void test1(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			cerr<<"Not manifold"<<endl;
			return ;
		}
	}
	cerr<<"The model passes manifold test"<<endl;

	std::vector<Mesh::VertexHandle> boundary;
	boundary.push_back(mesh.vertex_handle(0));
	mesh.data(mesh.vertex_handle(0)).set_distance(0);
	fast_marching (mesh, boundary);

	unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		cout<<mesh.data(v_it).get_distance()<<endl;
	}
}
void test2 ()
{
	//flip(double AB, double BC, double AC, double AD, double BD);
	//cout<< flip(sqrt(2.0f), 1.0f, 1, 1, 1) <<endl;

	
	//flip(double AB, double BC, double AC, double AD, double BD);
	//cout<< flip(sqrt(2.0f), 1.0f, 1, 1, 1) <<endl;
	//cout<<flip(sqrt(3.0f), 2, 1, 1, 1)<<endl;
}
void test3(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}

	Mesh::VertexVertexIter vv_it;
	vv_it= mesh.vv_iter( mesh.vertex_handle(0) );
	
	for (; vv_it; --vv_it)
	{
		cout<<vv_it.handle().idx()<<endl;
	}
	cout<<"Break"<<endl;
	cout<<vv_it.handle().idx()<<endl;
	--vv_it;
	cout<<vv_it.handle().idx()<<endl;
	//std::vector<Mesh::VertexHandle> boundary;
	//boundary.push_back(mesh.vertex_handle(0));
	//mesh.data(mesh.vertex_handle(0)).set_distance(0);
	//fast_marching (mesh, boundary);
}
void test4(char ** argv)
{
	//Test the function of flip;

	//int obtuse_process (Mesh & mesh, Mesh::VertexHandle va, Mesh::VertexHandle vb, Mesh::VertexHandle vc, double & tc)
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}


	mesh.data(mesh.vertex_handle(9)).set_state(ALIVE);
	mesh.data(mesh.vertex_handle(9)).set_distance(1);

//	mesh.data(mesh.vertex_handle(10)).set_state(ALIVE);
//	mesh.data(mesh.vertex_handle(10)).set_distance(0);

	mesh.data(mesh.vertex_handle(3)).set_state(ALIVE);
	mesh.data(mesh.vertex_handle(3)).set_distance(2);

	double tc;
	//obtuse_local_solver (mesh, mesh.vertex_handle(9), mesh.vertex_handle(10), mesh.vertex_handle(16), 1, tc);
	cout<<tc<<endl;


}

void test5_draw_isoline(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			cerr<<"Not manifold"<<endl;
			return ;
		}
	}
	cerr<<"The model passes manifold test"<<endl;

	std::vector<Mesh::VertexHandle> initial;
	//unsigned start_vertex_index = 50; // sphere /*Work badly*/
	unsigned start_vertex_index ; // sphere /*Work badly*/
	//unsigned start_vertex_index = 0;
	//unsigned start_vertex_index = 1086;   // Plane5
	//unsigned start_vertex_index = 383; // The cube1 start index
	Mesh::VertexHandle v_handle;

	start_vertex_index = 46;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(0);
	mesh.data(v_handle).set_state(ALIVE);
	initial.push_back(v_handle);

	start_vertex_index = 60;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(5);
	mesh.data(v_handle).set_state(ALIVE);
	initial.push_back(v_handle);

	fast_marching (mesh, initial);

	/*
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		mesh.data(v_it).set_state(FAR);
	}
	start_vertex_index = 60;
	v_handle = mesh.vertex_handle(start_vertex_index); 
	mesh.data(mesh.vertex_handle(start_vertex_index)).set_distance(0);
	mesh.data(v_handle).set_state(ALIVE);
	initial.clear();
	initial.push_back(v_handle);
	fast_marching (mesh, initial);
	*/


	

	double max_dis = 0;
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (max_dis < mesh.data(v_it).get_distance())
		{
			max_dis = mesh.data(v_it).get_distance();
		}
	}

	int n_lines = 100;
	double step = max_dis / (n_lines + 1);
	
	double isovalue = 0;
	std::vector<Mesh::Point> line;
	for (int i = 0; i < n_lines; ++i)
	{
		isovalue +=step;		
		draw_one_isoline(mesh, isovalue, line);
	}
	
	assert (line.size() % 2 == 0); 
	//outputoff(line);
	//outputobj(line);
	//generate_merged_mesh_isoline(mesh, line);
	generate_seperate_mesh_isoline(mesh, line);

	int count = 0;
	for (Mesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
	{
		std::cerr<<count++<<' '<<mesh.data(v_it).get_distance()<<std::endl;
	}

}



void test7_draw_geodesics_face_point (char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	//unsigned start_index = 404; //sphere
	unsigned start_index = 40; //sphere
	std::vector<Mesh::VertexHandle> initial;
	initial.push_back(mesh.vertex_handle(start_index));
	Mesh::Point end = mesh.point(mesh.vertex_handle(start_index));
	mesh.data(mesh.vertex_handle(start_index)).set_distance(0);
	fast_marching (mesh, initial);

	std::vector<Mesh::FaceHandle> end_faces;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(mesh.vertex_handle(start_index)); vf_it; ++vf_it)
	{
		end_faces.push_back(vf_it.handle());
		//std::cout<<vf_it.handle().idx()<<std::endl;
	}
	

	unsigned center_face_idx = 500;
	//unsigned center_face_idx = 50;

	Mesh::FaceHandle start_face = mesh.face_handle(center_face_idx);
	Mesh::Point start(0,0,0);
	for (Mesh::FaceVertexIter fv_it  = mesh.fv_iter(start_face); fv_it; ++fv_it)
	{
		
		start [0] +=mesh.point(fv_it)[0];
		start [1] +=mesh.point(fv_it)[1];
		start [2] +=mesh.point(fv_it)[2];
	}
	start [0] /= 3.0f;
	start [1] /= 3.0f;
	start [2] /= 3.0f;

	std::list<Mesh::Point> path;
	Mesh::HalfedgeHandle start_on_heh; 
	Mesh::VertexHandle start_point_handle;
	draw_geodesics(mesh,  start, start_point_handle, start_face, start_on_heh,  FACE_POINT, end, end_faces, 0.001, path);
	generate_seperate_mesh_geodesics(mesh, path);
}
void test8_draw_geodesics_edge_point (char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	unsigned start_index = 0; //sphere
	std::vector<Mesh::VertexHandle> initial;
	initial.push_back(mesh.vertex_handle(start_index));
	Mesh::Point end = mesh.point(mesh.vertex_handle(start_index));
	mesh.data(mesh.vertex_handle(start_index)).set_distance(0);
	fast_marching (mesh, initial);

	std::vector<Mesh::FaceHandle> end_faces;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(mesh.vertex_handle(start_index)); vf_it; ++vf_it)
	{
		end_faces.push_back(vf_it.handle());
		//std::cout<<vf_it.handle().idx()<<std::endl;
	}
	

	
	unsigned halfedge_idx = 500;
	//unsigned center_face_idx = 50;

	Mesh::HalfedgeHandle heh = mesh.halfedge_handle(halfedge_idx);
	Mesh::Point start(0,0,0);
	start = mesh.point(mesh.to_vertex_handle(heh));

	std::list<Mesh::Point> path;

	Mesh::VertexHandle start_point_handle;
	Mesh::FaceHandle start_face;
	draw_geodesics(mesh,  start, start_point_handle, start_face, heh, EDGE_POINT, end, end_faces, 0.01, path);
	generate_seperate_mesh_geodesics(mesh, path);
}
void test9_draw_geodesics_vertex_point (char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	/*Start for index 50 runs badly*/
	unsigned start_index = 50; //sphere
	std::vector<Mesh::VertexHandle> initial;
	initial.push_back(mesh.vertex_handle(start_index));
	Mesh::Point end = mesh.point(mesh.vertex_handle(start_index));
	mesh.data(mesh.vertex_handle(start_index)).set_distance(0);
	fast_marching (mesh, initial);

	std::vector<Mesh::FaceHandle> end_faces;
	for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(mesh.vertex_handle(start_index)); vf_it; ++vf_it)
	{
		end_faces.push_back(vf_it.handle());
		//std::cout<<vf_it.handle().idx()<<std::endl;
	}
	
	unsigned vertex_idx ;
	//unsigned center_face_idx = 50;

	
	Mesh::Point start(0,0,0);
	

	std::list<std::list<Mesh::Point> > path_group;
	std::list<Mesh::Point> path;
	Mesh::FaceHandle start_face;
	Mesh::HalfedgeHandle heh;
	

	//for (unsigned i = 1; i < 994; ++i) /*Start from 0 runs correctly*/
	for (unsigned i = 0; i < (mesh.n_vertices()); ++i)
	{
		if (i == start_index) continue;
		//Clear the face across count
		for (Mesh::FaceIter f_it = mesh.faces_begin(); f_it!= mesh.faces_end(); ++f_it)
		{
			mesh.data(f_it).across_count = 0;
		}
		vertex_idx = i;
		path.clear();
		Mesh::VertexHandle start_point_handle = mesh.vertex_handle(vertex_idx);
		start = mesh.point(start_point_handle);
		draw_geodesics(mesh,  start, start_point_handle, start_face, heh, VERTEX_POINT, end, end_faces, 0.001, path);
		path_group.push_back(path);
		std::cerr<<"Finishing draw geodesics "<<i<<std::endl;
	}
/*
	vertex_idx = 30;
	path.clear();
	start_point_handle = mesh.vertex_handle(vertex_idx);
	start = mesh.point(start_point_handle);
	draw_geodesics(mesh,  start, start_point_handle, start_face, heh, VERTEX_POINT, end, end_faces, 0.001, path);
	path_group.push_back(path);
*/
	generate_seperate_group_geodesics(mesh, path_group);
}
void test10_draw_geodesics_face_starting_point(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	/*Start for the centroid of face 50,20 runs badly*/
	/*100 cause an strange geodesics*/
	unsigned face_index = 20; 
	Mesh::FaceHandle face_handle = mesh.face_handle(face_index);
	Mesh::Point centroid (0,0,0);
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		//std::cerr<<"P: "<<mesh.point(fv_it)<<std::endl;
		centroid += mesh.point(fv_it);
	}
	centroid /= 3;
	//cerr<<centroid<<endl;
	//std::cout<<centroid<<std::endl;
	mesh.data(face_handle).point = centroid;
	//Prepare the initial point. 
	std::vector<Mesh::VertexHandle> initial;
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		initial.push_back(fv_it.handle());
		mesh.data(fv_it).set_distance((centroid - mesh.point(fv_it)).length());
		mesh.data(fv_it).set_state(ALIVE);
		//cout<<mesh.data(fv_it).get_distance()<<endl;
	}
	
	
	fast_marching(mesh, initial);

	Mesh::Point start;
	Mesh::Point end = centroid;
	std::vector<Mesh::FaceHandle> end_faces;
	end_faces.push_back(face_handle);

	//for (Mesh::FaceFaceIter ff_it = mesh.ff_iter(face_handle); ff_it; ++ff_it)
	//{
	//	end_faces.push_back(ff_it.handle());
	//}

	unsigned vertex_idx ;
	std::list<std::list<Mesh::Point> > path_group;
	std::list<Mesh::Point> path;
	Mesh::FaceHandle start_face;
	Mesh::HalfedgeHandle heh;

	/*
	Mesh::VertexHandle start_point_handle ;
	Mesh::FaceHandle start_face_handle = mesh.face_handle(632);

	start = Mesh::Point(-0.303814, -0.051478, -0.87735);
	draw_geodesics(mesh,  start, start_point_handle, start_face_handle, heh, FACE_POINT, end, end_faces, 0.005, path);
	path_group.push_back(path);
	*/

	
	for (unsigned i = 621; i < (mesh.n_vertices(), 622); ++i)
	{
		//if (i == start_index) continue;
		//Clear the face across count
		for (Mesh::FaceIter f_it = mesh.faces_begin(); f_it!= mesh.faces_end(); ++f_it)
		{
			mesh.data(f_it).across_count = 0;
		}
		vertex_idx = i;
		path.clear();
		Mesh::VertexHandle start_point_handle = mesh.vertex_handle(vertex_idx);
		start = mesh.point(start_point_handle);
		draw_geodesics(mesh,  start, start_point_handle, start_face, heh, VERTEX_POINT, end, end_faces, 0.005, path);
		path_group.push_back(path);
		std::cerr<<"Finishing draw geodesics "<<i<<std::endl;
	}
	


}
void test11_draw_isoline_face_starting_point(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	/*Start for the centroid of face 50,20 runs badly*/
	/*100 cause an strange geodesics*/
	unsigned face_index = 20; 
	Mesh::FaceHandle face_handle = mesh.face_handle(face_index);
	Mesh::Point centroid (0,0,0);
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		std::cerr<<"P: "<<mesh.point(fv_it)<<std::endl;
		centroid += mesh.point(fv_it);
	}
	centroid /= 3;
	cerr<<centroid<<endl;
	//std::cout<<centroid<<std::endl;
	mesh.data(face_handle).point = centroid;
	//Prepare the initial point. 
	std::vector<Mesh::VertexHandle> initial;
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		initial.push_back(fv_it.handle());
		mesh.data(fv_it).set_distance((centroid - mesh.point(fv_it)).length());
		mesh.data(fv_it).set_state(ALIVE);
		cerr<<"D:"<<mesh.data(fv_it).get_distance()<<endl;
	}
	
	
	fast_marching(mesh, initial);
//Generate the isoline part

	std::vector<Mesh::Point> line;
	double max_dis = 0;
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (max_dis < mesh.data(v_it).get_distance())
		{
			max_dis = mesh.data(v_it).get_distance();
		}
	}

	int n_lines = 100;
	double step = max_dis / (n_lines + 1);
	
	double isovalue = 0;
	for (int i = 0; i < n_lines; ++i)
	{
		isovalue +=step;		
		draw_one_isoline(mesh, isovalue, line);
	}
	
	assert (line.size() % 2 == 0); 
	generate_seperate_mesh_isoline(mesh, line);


}

void test12_randompoint(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	std::list<Mesh::Point> path;
	

	Random_points_mesh random_mesh(mesh);

	Mesh::Point point;
	Mesh::FaceHandle fh;
	
	for (int i = 0; i < 10000; ++i)
	{
		random_mesh.generate_point(point, fh);
		std::cout<<point<<std::endl;
		path.push_back(point);
	}
	
	generate_seperate_mesh_point_cloud(mesh, path);
	/*
	std::list<Mesh::Point> path;

	for (Mesh::FaceIter f_it = mesh.faces_begin(); f_it!= mesh.faces_end(); ++f_it)
	{
		unsigned int count = 0;
		Mesh::Point point[3];
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			point[count++] = mesh.point(fv_it.handle());
		}
		Triangle tri(point, f_it.handle());
		Mesh::Point r_p;
		tri.generate_random_point(r_p);
		path.push_back(r_p);
	}
	generate_seperate_mesh_geodesics(mesh, path);
	*/
	
}
void bug(char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	/*Start for the centroid of face 50,20 runs badly*/
	/*100 cause an strange geodesics*/
	unsigned face_index = 20; 
	Mesh::FaceHandle face_handle = mesh.face_handle(face_index);
	Mesh::Point centroid (0,0,0);
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		//std::cerr<<"P: "<<mesh.point(fv_it)<<std::endl;
		centroid += mesh.point(fv_it);
	}
	centroid /= 3;
	//cerr<<centroid<<endl;
	//std::cout<<centroid<<std::endl;
	mesh.data(face_handle).point = centroid;
	//Prepare the initial point. 
	std::vector<Mesh::VertexHandle> initial;
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		initial.push_back(fv_it.handle());
		mesh.data(fv_it).set_distance((centroid - mesh.point(fv_it)).length());
		mesh.data(fv_it).set_state(ALIVE);
		//cout<<mesh.data(fv_it).get_distance()<<endl;
	}
	
	
	fast_marching(mesh, initial);

	Mesh::Point start;
	Mesh::Point end = centroid;
	std::vector<Mesh::FaceHandle> end_faces;
	end_faces.push_back(face_handle);

	//for (Mesh::FaceFaceIter ff_it = mesh.ff_iter(face_handle); ff_it; ++ff_it)
	//{
	//	end_faces.push_back(ff_it.handle());
	//}

	unsigned vertex_idx ;
	std::list<std::list<Mesh::Point> > path_group;
	std::list<Mesh::Point> path;
	Mesh::FaceHandle start_face;
	Mesh::HalfedgeHandle heh;

	
	Mesh::VertexHandle start_point_handle ;
	//Mesh::FaceHandle start_face_handle = mesh.face_handle(632);
	//start = Mesh::Point(-0.303814, -0.051478, -0.87735);

	Mesh::FaceHandle start_face_handle = mesh.face_handle(270);
	start = Mesh::Point( -0.303816f, -0.0514625f, -0.877357f);
	draw_geodesics(mesh,  start, start_point_handle, start_face_handle, heh, FACE_POINT, end, end_faces, 0.005, path);
	path_group.push_back(path);

	generate_seperate_group_geodesics(mesh, path_group);

}

void test13_back_tracking_face_point (char ** argv)
{
	Mesh mesh;
	OpenMesh::IO::Options ropt, wopt;

	if (! OpenMesh::IO::read_mesh(mesh, argv[1], ropt))
	{
		std::cerr<<"Error loading mesh from file "<<argv[1]<<std::endl;
	}
	//cout<<mesh.n_vertices()<<endl;
	//unsigned n_vertices = mesh.n_vertices();
	//Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	
	//Mesh::VertexHandle v_handle = mesh.vertex_handle(0);
	//cout<<mesh.point (v_handle)<<endl;

	Mesh::VertexIter v_it, v_end (mesh.vertices_end());
	for (v_it = mesh.vertices_begin(); v_it != v_end; ++v_it)
	{
		if (! mesh.is_manifold(v_it) )
		{
			std::cerr<<"Not manifold"<<std::endl;
			return ;
		}
	}
	std::cerr<<"The model passes manifold test"<<std::endl;

	/*Start for the centroid of face 50,20 runs badly*/
	/*100 cause an strange geodesics*/
	unsigned face_index = 20; 
	Mesh::FaceHandle face_handle = mesh.face_handle(face_index);
	Mesh::Point centroid (0,0,0);
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		//std::cerr<<"P: "<<mesh.point(fv_it)<<std::endl;
		centroid += mesh.point(fv_it);
	}
	centroid /= 3;
	//cerr<<centroid<<endl;
	//std::cout<<centroid<<std::endl;
	mesh.data(face_handle).point = centroid;
	//Prepare the initial point. 
	std::vector<Mesh::VertexHandle> initial;
	for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++ fv_it)
	{
		initial.push_back(fv_it.handle());
		mesh.data(fv_it).set_distance((centroid - mesh.point(fv_it)).length());
		mesh.data(fv_it).set_state(ALIVE);
		//cout<<mesh.data(fv_it).get_distance()<<endl;
	}
	
	
	fast_marching(mesh, initial);

	Mesh::Point start;
	Mesh::Point end = centroid;
	std::vector<Mesh::FaceHandle> end_faces;
	end_faces.push_back(face_handle);

	//for (Mesh::FaceFaceIter ff_it = mesh.ff_iter(face_handle); ff_it; ++ff_it)
	//{
	//	end_faces.push_back(ff_it.handle());
	//}

	unsigned vertex_idx ;
	std::list<std::list<Mesh::Point> > path_group;
	std::list<Mesh::Point> path;
	Mesh::FaceHandle start_face;
	Mesh::HalfedgeHandle heh;
	
/*
	for (unsigned i = 1; i < (mesh.n_vertices(), 10); ++i)
	{
		//if (i == start_index) continue;
		//Clear the face across count
		for (Mesh::FaceIter f_it = mesh.faces_begin(); f_it!= mesh.faces_end(); ++f_it)
		{
			mesh.data(f_it).across_count = 0;
		}
		vertex_idx = i;
		path.clear();
		Mesh::VertexHandle start_point_handle = mesh.vertex_handle(vertex_idx);
		start = mesh.point(start_point_handle);
		draw_geodesics(mesh,  start, start_point_handle, start_face, heh, VERTEX_POINT, end, end_faces, 0.005, path);
		path_group.push_back(path);
		std::cerr<<"Finishing draw geodesics "<<i<<std::endl;
	}
*/
	for (unsigned face_id = 100; face_id < 120; ++face_id)
	{
		//Compute the backtracking starting Point
		Mesh::Point pc = Mesh::Point(0,0,0);
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(mesh.face_handle(face_id)); fv_it; ++ fv_it)
		{
			//std::cerr<<"P: "<<mesh.point(fv_it)<<std::endl;
			pc += mesh.point(fv_it);
		}
		pc /= 3;
		path.clear();
		Mesh::VertexHandle start_point_handle;
		start = pc;
		start_face = mesh.face_handle(face_id);
		draw_geodesics(mesh, start, start_point_handle, start_face, heh, FACE_POINT, end, end_faces, 0.005, path);
		path_group.push_back(path);
		std::cerr<<"Finishing draw geodesics"<<face_id<<std::endl;
	}
	generate_seperate_group_geodesics(mesh, path_group);
}

int main(int arc, char ** argv)
{

	//test1(argv);
	//test2();
	//test3(argv);

	//test4(argv);
	test5_draw_isoline(argv);
	//test7_draw_geodesics_face_point(argv);
	//test8_draw_geodesics_edge_point(argv);
	//test9_draw_geodesics_vertex_point(argv);
	//test10_draw_geodesics_face_starting_point(argv);
	//test11_draw_isoline_face_starting_point(argv);
	//test12_randompoint(argv);
	//bug(argv);
	// test13_back_tracking_face_point(argv);
	
	/*
	vector<double> range;
	range.push_back(0.0); //The first range;
	double length = 0.0f;
	Random_generator obj;
	int n_num = 10;
	for (int i = 0; i < n_num; ++i)
	{
		double segment = obj.generate_double();
		std::cout<<segment<<std::endl;
		length+=segment;
		range.push_back(length);
	}
	for (int i = 0; i < n_num + 1; ++i)
	{
		std::cout<<range[i]<<' ';
	}
	std::cout<<std::endl;
	double s = obj.generate_double(0.0f, range[n_num]);
	std::cout<<"search "<<s<<std::endl;
	//std::cout<<"Result "<<(*find_if(range.begin(), range.end()-1,  bind2nd(greater<double>(), s)))<<std::endl;
	std::cout<<"Result "<<(*upper_bound(range.begin(), range.end(),  s))<<std::endl;
	*/

    return 0;
}


